#include "robot_orientation.h"
#include <math.h>
using namespace cv;

RobotOrientation getRobotOrientation(Point red, Point green) {
	RobotOrientation robot;

	if(red.x == green.x){
		if(red.y > green.y){
			robot.angle = 0;
		}else{
			robot.angle = 180;
		}
	}else{
	  double phi = atan2( (double)(green.y - red.y), (double)(green.x - red.x))/M_PI*180;
		robot.angle = phi - 90;
		
		if(robot.angle<0)
			robot.angle += 360;
	}

	robot.point.x = (red.x + green.x)/2;
	robot.point.y = (red.y + green.y)/2;
	
	return robot;
}
